#include "M8010_control.h"
#include "crc_ccitt.h"
#include "ris_protocol.h"
#include "usart.h"
#include "pid.h"
//#include "ANO_send_data.h"
#include "math.h"

//速度控制 安全速度最大   8rad/s
//角度控制 输出轴角度    0-6.33*3.14*2=39.7524 rad
//输出力矩 绝对值最大    128

#define SATURATE(_IN, _MIN, _MAX) { \
 if (_IN < _MIN)                    \
 _IN = _MIN;                        \
 else if (_IN > _MAX)               \
 _IN = _MAX;                        \
 }

int modify_data(MOTOR_send *motor_s)
{
    motor_s->hex_len = 17;
    motor_s->motor_send_data.head[0] = 0xFE;
    motor_s->motor_send_data.head[1] = 0xEE;

//		SATURATE(motor_s->id,   0,    15);
//		SATURATE(motor_s->mode, 0,    7);
    SATURATE(motor_s->K_P,  0.0f,   25.599f);
    SATURATE(motor_s->K_W,  0.0f,   25.599f);
    SATURATE(motor_s->T,   -127.99f,  127.99f);
    SATURATE(motor_s->W,   -804.00f,  804.00f);
    SATURATE(motor_s->Pos, -411774.0f,  411774.0f);

    motor_s->motor_send_data.mode.id   = motor_s->id;
    motor_s->motor_send_data.mode.status  = motor_s->mode;
    motor_s->motor_send_data.comd.k_pos  = motor_s->K_P/25.6f*32768;
    motor_s->motor_send_data.comd.k_spd  = motor_s->K_W/25.6f*32768;
    motor_s->motor_send_data.comd.pos_des  = motor_s->Pos/6.2832f*32768;
    motor_s->motor_send_data.comd.spd_des  = motor_s->W/6.2832f*256;
    motor_s->motor_send_data.comd.tor_des  = motor_s->T*256;
    motor_s->motor_send_data.CRC16 = crc_ccitt(0, (uint8_t *)&motor_s->motor_send_data, 15);
    return 0;
}
int extract_data(MOTOR_recv *motor_r)
{
    if(motor_r->motor_recv_data.CRC16 !=
       crc_ccitt(0, (uint8_t *)&motor_r->motor_recv_data, 14)){
        motor_r->correct = 0;
        return motor_r->correct;
    }
    else//通过校验
    {
        motor_r->motor_id = motor_r->motor_recv_data.mode.id;
        motor_r->mode = motor_r->motor_recv_data.mode.status;
        motor_r->Temp = motor_r->motor_recv_data.fbk.temp;
        motor_r->MError = motor_r->motor_recv_data.fbk.MError;
        motor_r->W = ((float)motor_r->motor_recv_data.fbk.speed/256)*6.2832f ;
        motor_r->T = ((float)motor_r->motor_recv_data.fbk.torque) / 256;
        motor_r->Pos = 6.2832f*((float)motor_r->motor_recv_data.fbk.pos) / 32768;
        motor_r->footForce = motor_r->motor_recv_data.fbk.force;
        motor_r->correct = 1;

        angle_limit(motor_r);

        return motor_r->correct;
    }
}
HAL_StatusTypeDef SERVO_Send_recv(MOTOR_send *pData, MOTOR_recv *rData)
{
    uint16_t rxlen = 0;

    modify_data(pData);
    HAL_UART_Transmit(&huart6, (uint8_t *)pData, sizeof(pData->motor_send_data), 10);


    HAL_UARTEx_ReceiveToIdle(&huart6, (uint8_t *)rData, sizeof(rData->motor_recv_data), &rxlen, 10);

    if(rxlen == 0)
        return HAL_TIMEOUT;

    if(rxlen != sizeof(rData->motor_recv_data))
        return HAL_ERROR;

    if(rData->motor_recv_data.head[0] == 0xFD && rData->motor_recv_data.head[1] == 0xEE)//校验包头
    {
        rData->correct = 1;
        extract_data(rData);
        return HAL_OK;
    }

    return HAL_ERROR;
}

void M8010_pare_init(MOTOR_send *send1,MOTOR_send *send2)
{
    send1->id=0;
    send1->mode=1;
    send1->T=0;
    send1->W=0;
    send1->Pos=0;
    send1->K_P=0;
    send1->K_W=0.08f;   //力矩模式 大臂

    send2->id=1;
    send2->mode=1;
    send2->T=0;
    send2->W=0;
    send2->Pos=0;
    send2->K_P=0;
    send2->K_W=0.08f;   //力矩模式 小臂
}

void angle_limit(MOTOR_recv *recv)
{
    int16_t r_cnt;
    r_cnt = recv->Pos / max_rad;
    if(recv->Pos<0){
        r_cnt=r_cnt-1;
    }
    recv->out_angle=recv->Pos-max_rad*r_cnt;
}

//速度控制 安全速度最大   8rad/s
//角度控制 输出轴角度    0-6.33*3.14*2=39.7524 rad
//输出力矩 绝对值最大    128

void M8010_PID_Init(MOTOR_ctrl *motor1,M8010_CtrlMode mode1,MOTOR_ctrl *motor2,M8010_CtrlMode mode2)
{
    motor1->mode=mode1;
    motor1->Kp_V=0.01;
    motor1->Ki_V=0;
    motor1->Kd_V=0;
    motor1->Kp_P=0;
    motor1->Ki_P=0;
    motor1->Kd_P=0;
    motor1->Max_iout_V=0;
    motor1->Max_out_V=0;
    motor1->Max_iout_P=0;
    motor1->Max_out_P=0;
    motor1->DeadBand_V=0;
    motor1->DeadBand_P=0;

    motor2->mode=mode2;
    motor2->Kp_V=0;
    motor2->Ki_V=0;
    motor2->Kd_V=0;
    motor2->Kp_P=0;
    motor2->Ki_P=0;
    motor2->Kd_P=0;
    motor2->Max_iout_V=0;
    motor2->Max_out_V=0;
    motor2->Max_iout_P=0;
    motor2->Max_out_P=0;
    motor2->DeadBand_V=0;
    motor2->DeadBand_P=0;

}



float M8010_PID_Calc(MOTOR_ctrl *PID,float measure_V,float measure_P,float target)
{
    float V_Pout=0,V_Iout=0,V_Dout=0;

    if(PID == NULL)
        return 0;

    if(PID->mode==M8010_Serial){

    }
    else{
        PID->error_V[1]=PID->error_V[0];
        PID->error_V[0]=(target-measure_V);
    }   //计算完速度环error

    if(fabsf(PID->error_V[0])>PID->DeadBand_V||PID->DeadBand_V==0) {
        V_Pout = PID->Kp_V * PID->error_V[0];
        V_Iout += PID->Ki_V * PID->error_V[0];
        V_Dout = PID->Kd_V * (PID->error_V[0] - PID->error_V[1]);
        LimitMax(V_Iout, PID->Max_iout_V);
        PID->OUT_V = V_Pout + V_Iout + V_Dout;
        LimitMax(PID->OUT_V, PID->Max_out_V);
    } else{
        PID->OUT_V=0;
    }

    return PID->OUT_V;
}


void M8010_P_PID_Init(PID_ARM *motor1,PID_ARM *motor2)
{
    motor1 ->Kp = 8;//4
    motor1 ->Ki = 0;
    motor1 ->Kd = 0;
    
    motor1 ->Pout = 0;   
    motor1 ->Iout = 0;
    motor1 ->Dout = 0;
 
    
    motor1 ->error[0] = 0;
    motor1 ->error[1] = 0;
    motor1 ->error[2] = 0;
  
    motor1 ->out = 0;
    motor1 ->Max_iout = 0;
    motor1 ->Max_out = 15;//15
    motor1 ->Deadband = 0;
    
    motor2 ->Kp = 8;  //4
    motor2 ->Ki = 0;
    motor2 ->Kd = 0;
        
    motor2 ->Pout = 0;   
    motor2 ->Iout = 0;
    motor2 ->Dout = 0;
    
    motor2 ->error[0] = 0;
    motor2 ->error[1] = 0;
    motor2 ->error[2] = 0;
  
    motor2 ->out = 0;
    motor2 ->Max_iout = 0;
    motor2 ->Max_out = 15;//8
    motor2 ->Deadband = 0;
}
float PID_P(PID_ARM *motor,float target,float real)
{

    motor ->error[2] = motor -> error[0] - motor -> error[1];
    motor ->error[1] = motor -> error [0];
    motor ->error[0] = target - real;
    
    motor ->Pout = motor -> error[0] * motor ->Kp;
    motor ->Iout += motor -> error[0] * motor ->Ki; 
    motor ->Dout = motor -> error[2] * motor ->Kd;
    
    LimitMax(motor ->Iout,motor -> Max_iout);
    motor ->out = motor ->Pout + motor ->Iout + motor ->Dout;
    LimitMax(motor ->out,motor->Max_out);
    
    return motor -> out;
}

